from tensorflow.keras.models import load_model
import sys
import numpy as np
import matplotlib.pyplot as plt

import airsim
#from models import Net
MODEL_PATH = 'model/models/model_model.14-0.0002725.h5'


model = load_model(MODEL_PATH)
#connected to airsim
client = airsim.CarClient()
client.confirmConnection()
client.enableApiControl(True)
car_controls = airsim.CarControls()

#set default status
car_controls.steering = 0
car_controls.throttle = 0
car_controls.brake = 0

image_buf = np.zeros((1, 59, 255, 3))
state_buf = np.zeros((1, 4))

#get image from airsim
def get_image():
    responses = client.simGetImages([airsim.ImageRequest("0", airsim.ImageType.Scene, False, False)])
    response = responses[0]
    image_1d = np.fromstring(response.image_data_uint8, dtype=np.uint8)
    image_rgb = image_1d.reshape(response.height, response.width, 3)
    plt.imshow(image_rgb)
    plt.show()
    return image_rgb[76:135, 0:255, 0:3].astype(float)

def navigate_car():
    car_state = client.getCarState()

    if(car_state.speed < 5):
        car_controls.throttle = 1.0
    else:
        car_controls.throttle = 0.0

    image_buf[0] = get_image()
    state_buf[0] = np.array([car_controls.steering, car_controls.throttle, car_controls.brake, car_state.speed])
    model_output = model.predict([image_buf, state_buf])
    car_controls.steering = round(0.5 * float(model_output[0][0]), 2)

    print('Sending steering = {0}, throttle = {1}'.format(car_controls.steering, car_controls.throttle))

    client.setCarControls(car_controls)


while(True):
    navigate_car()
